#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <std_msgs/String.h>
#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <math.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/filter_indices.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/features/normal_3d_omp.h>   //法线


void pointCloudCb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){

    pcl::PCLPointCloud2 pcl_pc2_temp;
    pcl_conversions::toPCL(*cloud_msg,pcl_pc2_temp);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_front(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_in_front->clear();
    pcl::fromPCLPointCloud2(pcl_pc2_temp,*cloud_in_front);

    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    ne.setNumberOfThreads(12);  // 手动设置线程数，否则提示错误
    ne.setInputCloud (cloud_in_front);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    ne.setSearchMethod (tree);
    // Output datasets
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    // Use  neighbors
    ne.setKSearch (20);
    ne.setViewPoint(0,0,0);
    // Compute the features
    ne.compute (*cloud_normals);

    pcl::visualization::PCLVisualizer viewer("NormalEstimation");
    viewer.setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloudHandler
            (cloud_in_front, 0, 255, 0);
    viewer.addPointCloud(cloud_in_front, cloudHandler, "cloud_Normal");
    viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_in_front, cloud_normals,4,0.07);
    std::cout << "法向估计完成！" << std::endl;
    std::cout << cloud_in_front->size() << std::endl;
    std::cout << cloud_normals->size() << std::endl;
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }

}


int main (int argc, char** argv)
{   // Initialize ROS
    ros::init (argc, argv, "normalEstimate");//声明节点的名称
    ros::NodeHandle nh;

    // Create a ROS subscriber for the input point cloud
    // 为接受点云数据创建一个订阅节点
    ros::Subscriber sub_front = nh.subscribe<sensor_msgs::PointCloud2> ("ground_cloud/front", 1, pointCloudCb);

    // Create a ROS publisher for the output point cloud
    //创建ROS的发布节点
//    pub_AllPointCloud = nh.advertise<sensor_msgs::PointCloud2> ("AllPointCloudOutput", 1);
    // Spin
    ros::spin ();
}


